#include "getAmmo_cv.h"

using namespace cv;
using namespace std;
/**
 * @brief FindAmmo的初始化函数
 * -通过特征匹配来寻找目标的一个类
 * @author
 * -谭咏雯
 * -杨泽霖
**/
FindAmmo::FindAmmo()
{
	/*** 设置“疾风”特征点检测器的参数 ***/
	//金字塔层数越高，检测到的点越多，速度也越慢
	//金字塔层数对速度的影响极其明显
	akaze->setNOctaves(3);		//高斯金字塔层数
	akaze->setNOctaveLayers(4); //金字塔层数
	//因为弹药箱不会旋转或翻转，关掉旋转不变性可以提高匹配成功的概率
	akaze->setDescriptorType(AKAZE::DESCRIPTOR_MLDB_UPRIGHT); //去掉特征点描述的旋转不变性
	/*** 设置各个vector预留的内存量 ***/
	//就算不设置，程序也能正常运行
	//但设置之后程序运行速度会加快
	//数字与kp.xml中kp1的数量相同
	kpts_17_42.reserve(KP_17_42_CNT);
	kp42.reserve(KP42_CNT);
	matched_17_42.reserve(KP_17_42_CNT);
	matched2.reserve(KP_17_42_CNT);
	nn_matches.reserve(KP_17_42_CNT);
	inlier_matches.reserve(KP_17_42_CNT);
	/*** 创建汉明距离匹配器 ***/
	//特征点描述是大约61个二进制数字
	//通过计算汉明距离，可以判断两个特征点是否匹配
	//汉明距离就是两个二进制数字中有几个数字不一样
	//比如说101101和101000中，有两个数字不一样，他们的汉明距离就是2
	matcher.create(NORM_HAMMING);
	/*** 读取kp.xml中的kpts1和desc1 ***/
	FileStorage fs("kp_17_42.xml", FileStorage::READ);
	for (size_t i = 0; i < KP_17_42_CNT; i++)
	{
		fs["kpts_17_42_" + to_string(i)] >> kpts_17_42[i];
	}
	fs["desc_17_42"] >> desc_17_42;
	for (size_t i = 0; i < KP42_CNT; i++)
	{
		fs["kpts" + to_string(i)] >> kp42[i];
	}
	fs["desc"] >> desc42;

	fs.release();
}

/**
 * @brief 将“关键点集”转换成“普通点集”
 * @param keypoints 输入的关键点集 
 * @param bias 偏移
 * -对于未取ROI和模板图片的点集kpts_17_42，bias缺省即可。
 * -对于kpts2，bias输入ROI左上角的点，即ROI.tl()。
 * -这是因为取了ROI之后点的位置错位了，要先校正回来。
 * @return 输出的普通点集
 * @author
 * -杨泽霖
 * -谭咏雯
**/
vector<Point2f> FindAmmo::Points(vector<KeyPoint> keypoints, Point2f bias = Point(0, 0))
{
	vector<Point2f> res;
	for (unsigned i = 0; i < keypoints.size(); i++)
	{
		res.push_back(keypoints[i].pt + bias);
	}
	return res;
}

/**
 * @brief 清除所有vector
 * -如果发现运行时间越长，速度越慢，或者内存一直增加，那就说明你忘记释放了
 * -比较耗时的操作，一定要放在数据发出去之后
 * @author
 * -杨泽霖
 * -谭咏雯
**/
void FindAmmo::ReleaseAll()
{
	imshow("result", img2);
	waitKey(1);
	matched_17_42.clear();
	matched2.clear();
	inlier_matches.clear();
	inliers2.clear();
	nn_matches.clear();
	inlier_matches.clear();
}

/**
 * @brief 检测不到任何东西
 * -输出原因，然后调用释放函数。
 * @author
 * -杨泽霖
 * -谭咏雯
**/
void FindAmmo::DetectNothing()
{
	cout << "**********************************************************" << endl;
	cout << "# Keypoints _17_42:\t" << kpts_17_42.size() << endl;	//模板点集，固定是KP_17_42_CNT
	cout << "# Keypoints 2:\t" << kpts2.size() << endl;	//在ROI中监测到的特征点，正常都是50以上
	cout << "# Matches:    \t" << matched_17_42.size() << endl; //匹配成功的特征点，正常都是30以上
	tol++;
	cout << "# tol:         \t" << tol << endl;
	cout << endl;
	ROI = Rect(Point(0, ROI_Y_UP), Point(640, ROI_Y_DOWN));
	send_data.Flag=0;
	ReleaseAll();
}

int FindAmmo::getAmmo_17_42(Mat frame)
{
	img2 = frame;

	double t = (double)getTickCount();
	Mat img2_ROI = img2(ROI);
	cvtColor(img2_ROI, img2_ROI, CV_BGR2GRAY);
	akaze->detectAndCompute(img2_ROI, noArray(), kpts2, desc2);
	if (kpts2.empty())
	{
		DetectNothing();
		return 0;
	}
	//画出ROI在	哪里
	rectangle(img2, ROI, Scalar(0, 255, 0));
	//用汉明距离匹配器、K近邻算法匹配特征点
	//前面已经说过什么是汉明距离了，记得desc_17_42和desc2都是二进制数字
	//最后一个参数指的是，给每个desc_17_42匹配2个最接近的desc2
	//返回的时候是按照距离接近程度排序的
	matcher.knnMatch(desc_17_42, desc2, nn_matches, 2);
	for (size_t i = 0; i < nn_matches.size(); i++)
	{
		DMatch first = nn_matches[i][0];
		float dist_17_42 = nn_matches[i][0].distance;
		float dist2 = nn_matches[i][1].distance;
		//如果最像desc1的特征点比第二接近的明显好足够多，我们才接受
		//这是防止当前帧出现一个，长得比正确的特征点还像模板的特征点
		//这也是为什么不能同时检测两个弹药箱的原因
		//这个0.8是一个可以调整的参数
		//如果还是理解不了打电话问我
		if (dist_17_42 <  dist2)
		{
			matched_17_42.push_back(kpts_17_42[first.queryIdx]);
			matched2.push_back(kpts2[first.trainIdx]);
		}
	}
	Mat inlier_mask, homography;
	//只有超过10个点匹配成功才继续（这个阈值已经很低了，正常都是50左右的）
	if (matched_17_42.size() >= 7)
	{
		//根据匹配到的特征点查找homography矩阵
		//算法是RANSAC，原理你应该清楚
		//最大迭代次数2000次，提高这个数字能提高精度，同时降低速度
		//有0.9（90%）的点是内点就直接结束迭代
		homography = findHomography(Points(matched_17_42), Points(matched2, ROI.tl()),
									RANSAC, 3, inlier_mask, 2000, 0.8);
	}
	else
	{
		DetectNothing();
		return 0;
	}

	//数内点数量
	int inlier_cnt = 0;
	for (size_t i = 0; i < inlier_mask.rows; i++)
	{
		if (inlier_mask.at<uchar>(i))
			inlier_cnt++;
	}

	//只有超过10个内点成功才继续（这个阈值已经很低了，正常都是50左右的）
	if (inlier_cnt < 7 || homography.empty())
	{
		DetectNothing();
		return 0;
	}

	double *homo0 = homography.ptr<double>(0);
	double *homo1 = homography.ptr<double>(1);
	double *homo2 = homography.ptr<double>(2);
	double scale = homo2[0] + homo2[1] + homo2[2];
	double trans_X = (homo0[0] + homo0[1]) / scale;
	double trans_Y = (homo1[0] + homo1[1]) / scale;

	//判断homography矩阵是否异常
	//起点在原点的矢量(1,1)左乘homography矩阵后，再把起点平移回原点，矢量变换到(trans_X, trans_Y)
	//trans_X可以看作是横坐标的放大系数，trans_Y是纵坐标放大系数
	//如果trans_X或trans_Y小于0，则说明出现翻转或者旋转，则判定为homograpgy异常
	if (trans_X < 0.1 || trans_X > 3.00 || trans_Y < 0.1 || trans_Y > 3.00)
	{
		DetectNothing();
		return 0;
	}

	//将X数字获取
	double new_X = (homo0[0] * 320 + homo0[1] * 240 + homo0[2]) / (homo2[0] * 320 + homo2[1] * 240 + homo2[2]);

	/*!!! 在此处发出串口指令 !!!*/

	//计算并输出时间
	t = ((double)getTickCount() - t) / getTickFrequency();
	cout << "Times:" << int(t*1000) << endl;

	//画出X数字所在位置
	line(img2, Point(new_X, 0), Point(new_X, 479), Scalar(255), 2);
	//这里只是防止ROI超出画面。注意！不要错误地把这个new_X发出去！！！
	if (new_X < 90)
		new_X = 90;
	if (new_X > 439)
		new_X = 439;
	//选取新的ROI
	cout << "x,matched_17_42.size():" << new_X-320-69-3.8<< "，" << matched_17_42.size() << endl;
	send_data.Flag=2;
send_data.X=new_X-320-69-3.8;
	ROI = Rect(Point(new_X - 85, ROI_Y_UP), Point(new_X + 180, ROI_Y_DOWN));

	ReleaseAll();

	return 0;
}

int FindAmmo::getAmmo_42(Mat frame)
{
	img2 = frame;

	double t = (double)getTickCount();
	Mat img2_ROI = img2(ROI);
	cvtColor(img2_ROI, img2_ROI, CV_BGR2GRAY);
	akaze->detectAndCompute(img2_ROI, noArray(), kpts2, desc2);
	if (kpts2.empty())
	{
		DetectNothing();
		return 0;
	}
	//画出ROI在	哪里
	rectangle(img2, ROI, Scalar(0, 255, 0));
	//用汉明距离匹配器、K近邻算法匹配特征点
	//前面已经说过什么是汉明距离了，记得desc_17_42和desc2都是二进制数字
	//最后一个参数指的是，给每个desc_17_42匹配2个最接近的desc2
	//返回的时候是按照距离接近程度排序的
	matcher.knnMatch(desc42, desc2, nn_matches, 2);
	for (size_t i = 0; i < nn_matches.size(); i++)
	{
		DMatch first = nn_matches[i][0];
		float dist_17_42 = nn_matches[i][0].distance;
		float dist2 = nn_matches[i][1].distance;
		//如果最像desc1的特征点比第二接近的明显好足够多，我们才接受
		//这是防止当前帧出现一个，长得比正确的特征点还像模板的特征点
		//这也是为什么不能同时检测两个弹药箱的原因
		//这个0.8是一个可以调整的参数
		//如果还是理解不了打电话问我
		if (dist_17_42 <  dist2)
		{
			matched_17_42.push_back(kp42[first.queryIdx]);
			matched2.push_back(kpts2[first.trainIdx]);
		}
	}
	Mat inlier_mask, homography;
	//只有超过10个点匹配成功才继续（这个阈值已经很低了，正常都是50左右的）
	if (matched_17_42.size() >= 7)
	{
		//根据匹配到的特征点查找homography矩阵
		//算法是RANSAC，原理你应该清楚
		//最大迭代次数2000次，提高这个数字能提高精度，同时降低速度
		//有0.9（90%）的点是内点就直接结束迭代
		homography = findHomography(Points(matched_17_42), Points(matched2, ROI.tl()),
									RANSAC, 3, inlier_mask, 2000, 0.8);
	}
	else
	{
		DetectNothing();
		return 0;
	}

	//数内点数量
	int inlier_cnt = 0;
	for (size_t i = 0; i < inlier_mask.rows; i++)
	{
		if (inlier_mask.at<uchar>(i))
			inlier_cnt++;
	}

	//只有超过10个内点成功才继续（这个阈值已经很低了，正常都是50左右的）
	if (inlier_cnt < 7 || homography.empty())
	{
		DetectNothing();
		return 0;
	}

	double *homo0 = homography.ptr<double>(0);
	double *homo1 = homography.ptr<double>(1);
	double *homo2 = homography.ptr<double>(2);
	double scale = homo2[0] + homo2[1] + homo2[2];
	double trans_X = (homo0[0] + homo0[1]) / scale;
	double trans_Y = (homo1[0] + homo1[1]) / scale;

	//判断homography矩阵是否异常
	//起点在原点的矢量(1,1)左乘homography矩阵后，再把起点平移回原点，矢量变换到(trans_X, trans_Y)
	//trans_X可以看作是横坐标的放大系数，trans_Y是纵坐标放大系数
	//如果trans_X或trans_Y小于0，则说明出现翻转或者旋转，则判定为homograpgy异常
	if (trans_X < 0.1 || trans_X > 3.00 || trans_Y < 0.1 || trans_Y > 3.00)
	{
		DetectNothing();
		return 0;
	}

	//将X数字获取
	double new_X = (homo0[0] * 320 + homo0[1] * 240 + homo0[2]) / (homo2[0] * 320 + homo2[1] * 240 + homo2[2]);

	/*!!! 在此处发出串口指令 !!!*/

	//计算并输出时间
	t = ((double)getTickCount() - t) / getTickFrequency();
	cout << "Times:" << int(t*1000) << endl;

	//画出X数字所在位置
	line(img2, Point(new_X, 0), Point(new_X, 479), Scalar(255), 2);
	//这里只是防止ROI超出画面。注意！不要错误地把这个new_X发出去！！！
	if (new_X < 90)
		new_X = 90;
	if (new_X > 439)
		new_X = 439;
	//选取新的ROI
	cout << "x,matched_17_42.size():" << new_X-320-69-3.8<< "，" << matched_17_42.size() << endl;
	send_data.Flag=2;
send_data.X=new_X-320-69-3.8;
	ROI = Rect(Point(new_X - 85, ROI_Y_UP), Point(new_X + 180, ROI_Y_DOWN));

	ReleaseAll();

	return 0;
}
